
#ifndef ROBOTKIN_PIDCONTROLLER
#define ROBOTKIN_PIDCONTROLLER

class PIDController {
 public:
  PIDController(double Kp, double Ki, double Kd);
  ~PIDController();

  void setKp(double Kp) { Kp_ = Kp; }
  void setKi(double Ki) { Ki_ = Ki; }
  void setKd(double Kd) { Kd_ = Kd; }
  void setAntiWindup(double min, double max) { min_integral_ = min; max_integral_ = max; }
  
  double compute(double error, double timeStep);

 private:
  double Kp_;
  double Ki_;
  double Kd_;
  
  double integral_;
  double min_integral_;
  double max_integral_;
  double last_state_;
};

inline PIDController::PIDController(double Kp, double Ki, double Kd) {
  Kp_ = Kp;
  Ki_ = Ki;
  Kd_ = Kd;
  integral_ = 0.0;
  last_state_ = 0.0;          // Todo set to state on first tick?
  min_integral_ = -1e23;      // Essentially Negative Infinity
  max_integral_ = 1e23;       // Essentially Positive Infinity
}

inline PIDController::~PIDController() {
  // destruct
}

// BASED ON http://brettbeauregard.com/blog/2011/04/improving-the-beginner%E2%80%99s-pid-reset-windup/
inline double PIDController::compute(double state, double setpoint) {
  
  double error = setpoint - state;
  while(error > +M_PI/2) error -= M_PI;
  while(error < -M_PI/2) error += M_PI;
  
  double pTerm = Kp_*error;
  
  integral_ += Ki_*error;
  
  double dState = state-last_state_;
  while(dState > +M_PI/2) dState -= M_PI;
  while(dState < -M_PI/2) dState += M_PI;
  
  double dTerm = Kd_*(dState);
  
  // Apply anti-windup:
  if(integral_ > max_integral_) integral_= max_integral_;
  else if(integral_ < min_integral_) integral_= min_integral_;
  
  double output = pTerm + integral_ - dTerm;

  //std::cout << "P:" << pTerm << "  I:" << integral_ << "  D:" << dTerm << "\n";
  
  last_state_ = state;
  return output;
}

#endif
